# Sap-Log.tcl - Simulation for ArduPilot File and logging functionality
# 
# Copyright (C) 2010  Paul Hopkins  email:- hopslink@gmail.com
#
#    This program is free software: you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation, either version 3 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program.  If not, see <http://www.gnu.org/licenses/>.
#


# select_save_file
# gets filename from user via GUI  
#
# Arguments: 
#  - Extension types list
#  - Default extension type
#
# returns: - selected filename as string
#
proc ::sap::select_save_file {filetypes extension} {
        variable S
	
        set filename [tk_getSaveFile -filetypes $filetypes   \
                    -defaultextension $extension                 \
                    -initialdir pwd                  ]
        if { $filename != "" } {
                if { [file exists $filename] && ! [file isfile $filename] } {
			set warn [tk_messageBox -title "Error" \
			 	-message "File \"$filename\" is not a regular file" \
			 	-type ok ]                       
                }
		
        return $filename
        }	
}

# saveFile - save telemetry data to log file

proc ::sap::saveFile {} {
	variable S
	
	# get and save a filename for the logfile 
	set S(fileName) [::sap::select_save_file $S(log.FileTypes) ".log"]	;
	
	# check for user cancel 
	if { $S(fileName) == "" } {
            	return
        }
	
	update
	
	set chan [open $S(fileName) w]
        fconfigure $chan -buffering none 	;# -translation binary
	
	foreach x $S(log.Data) {
		puts $chan $x		
	}
        flush $chan
        close $chan	
}

proc ::sap::loadFile {} {
	variable S
	set filename [tk_getOpenFile -filetypes $S(log.FileTypes)   \
                    -defaultextension .log                   \
                    -initialdir pwd                  ]
        if { $filename != "" } {
                if { [file exists $filename] && ! [file isfile $filename] } {
                        ::scope::error_ret "File \"$filename\" is not a regular file"
                }
        set S(fileName) $filename
		
	update
		
	set infile [open $S(fileName) r]
        fconfigure $infile -buffering none 	;# -translation binary
	
		
	set S(log.Data) [list]   ;# !CLEARS ANY EXISTING DATA
	# read in data 'til EOF is read
	while { [gets $infile line] >= 0 } {
    		lappend S(log.Data) $line
	}        
        close $infile		
        }
}

# exportKml
# outputs lat,lon,alt data for flight as Google Earth .kml
# 
proc ::sap::exportKml {} {
	variable S
	
	# test and return if there is no data to export
	if {[llength $S(log.Data)] <= 0} {
		set answer [tk_messageBox -title "Error" \
                	-message "No data available to export" \
                	-type ok ]
		return
	}
	
	# get file name
	set filename [::sap::select_save_file $S(log.FileTypesKml) ".kml"]
	
	# check for user cancel 
	if { $filename == "" } {
            	return
        }
	
	update
	
	set kmlExportData [list]
	set kmlWaypointData [list]
	
	set referenceTime 0 		;# for calculating time offsets
	# step through log data
	foreach x $S(log.Data) {
		
		#WAYPOINTS
		# find correct lines from AP
		if {[string range $x 0 4] == "home:"} {
			#lose the ":"
			#set tempx "home[string range $x 5 end]"
			lappend kmlWaypointData [::sap::getKmlWaypoint "HOME[string range $x 5 end]"]
		}
		
		if {[string range $x 0 3] == "wp #"} {
			#lose the "#"
			#set tempx "WP [string range $x 4 end]"
			lappend kmlWaypointData [::sap::getKmlWaypoint "WP [string range $x 4 end]"]
		}
		
		# COORDINATES
		# find correct lines from AP
		if {[string range $x 0 2] != "!!!"} {
			continue
		}
		
		# get time data from string
		set inTimeStartIndex [string last "," $x]
		if {$inTimeStartIndex > 90} {		;# exclude malformed packets
			
        		set inTime [string range $x [incr inTimeStartIndex] end]
        		
			if { [expr { abs ( $inTime - $referenceTime )} ] > $S(log.KmlDeltaMilliseconds) } { 
        			
        			set referenceTime $inTime
				# extract and format data
				set packetList [split $x ",:"]
				set latData [lindex $packetList 1]
                        	set lonData [lindex $packetList 3]
				set altData [lindex $packetList 9]
				
				# create a list for data logging
                            	set toLogData [list]
                            	# scale data and add to logging list
                            	lappend toLogData [expr {$lonData / $S(scaleAPLocationInt2Float)}]
                            	lappend toLogData [expr {$latData / $S(scaleAPLocationInt2Float)}]
                            	lappend toLogData $altData
                            	# convert list to csv string
                            	set logString [join $toLogData ","]
                            	# save the string for logging
                            	lappend kmlExportData $logString								
        		} 
		}		
	}
	# open file
	set chan [open $filename w]
        fconfigure $chan -buffering none 	;# -translation binary	
        
	# write generic header
	set kmlHeader [::sap::getKmlHeader]
	puts $chan $kmlHeader
	
	# write coordinate data to file
	foreach i $kmlExportData {
		puts $chan "\t\t\t\t\t$i"		
	}
	
	# write co-ordinate footer data
	set kmlCoordFooter [::sap::getKmlCoordinateFooter]
	puts $chan $kmlCoordFooter
	
	#write waypoint data
	foreach j $kmlWaypointData {
		puts $chan "$j"		
	}
	
	# write footer data
	set kmlFooter [::sap::getKmlFooter]
	puts $chan $kmlFooter
	
	# close file
	flush $chan
        close $chan	
}

# getKmlWaypoint
# Formats AP waypoint data to a kml placemarker
#
# Arguments:
#  wpData - AP initialisation waypoint printout string
#
# Returns:
#  formatted "kml" string to add waypoint  
#
proc ::sap::getKmlWaypoint {wpData} {
	variable S
	# split to waypoint data to components
	set wpTempList [split $wpData ",\t" ]
	
	if {[llength $wpTempList] == 5  } {
		set wpName [lindex $wpTempList 0]
#		set wpLat [expr {[lindex $wpTempList 1] / $S(scaleAPLocationInt2Float)}]
#		set wpLon [expr {[lindex $wpTempList 2] / $S(scaleAPLocationInt2Float)}]
		set wpLat [expr {[lindex $wpTempList 1] / 10000000.0}]
		set wpLon [expr {[lindex $wpTempList 2] / 10000000.0}]
		set wpAlt [expr {[lindex $wpTempList 3] / 100.0}]
		#set wpAlt [lindex $wpTempList 3]
		
		set kmlString "<Placemark> \n \
		<name>$wpName</name>\n \
		<styleUrl>#waypointmarker</styleUrl>\n \
		<Point>\n \
			<altitudeMode>absolute</altitudeMode>\n \
			<coordinates>$wpLon,$wpLat,$wpAlt</coordinates>\n \
		</Point>\n \
		</Placemark>"
		
		return $kmlString
	} else {
		#fail
		return ""
	}
}


# getKmlHeader
# returns a string containing formatted .xml data
# return data is a 'stock' Google Earth .kml header
# .kml file made up of 'header', co-ordinate data and 'footer' 
#TODO - provide user selectable linestyles
proc ::sap::getKmlHeader {} {
	set kmlHeader "<?xml version=\"1.0\" encoding=\"UTF-8\"?> \n \
		<kml xmlns=\"http://www.opengis.net/kml/2.2\"> \n \
  		\t<Document> \n \
    		\t\t<name>Paths</name> \n \
		\t\t<Style id=\"waypointmarker\"> \n \
		\t\t\t<IconStyle> \n \
		\t\t\t\t<scale>0.7</scale> \n \
		\t\t\t\t<Icon> \n \
		\t\t\t\t\t<href>http://maps.google.com/mapfiles/kml/pushpin/ylw-pushpin.png</href> \n \
		\t\t\t\t</Icon> \n \
		\t\t\t\t<hotSpot x=\"20\" y=\"2\" xunits=\"pixels\" yunits=\"pixels\"/> \n \
		\t\t\t</IconStyle> \n \
		\t\t</Style> \n \
    		\t\t<Style id=\"redLineTransBluePoly\">\n \
      		\t\t\t<LineStyle>\n \
		\t\t\t\t<color>7f0000ff</color>\n \
		\t\t\t\t<width>3</width>\n \
		\t\t\t</LineStyle>\n \
		\t\t\t<PolyStyle>\n \
		\t\t\t\t<color>30000080</color>\n \
		\t\t\t</PolyStyle>\n \
		\t\t</Style>\n \
		\t\t<Placemark>\n \
		\t\t\t<name>Absolute Extruded</name>\n \
		\t\t\t<description>Transparent blue wall with red outlines</description>\n \
		\t\t\t<styleUrl>#redLineTransBluePoly</styleUrl>\n \
		\t\t\t<LineString>\n \
		\t\t\t\t<extrude>1</extrude>\n \
		\t\t\t\t<tessellate>1</tessellate>\n \
		\t\t\t\t<altitudeMode>absolute</altitudeMode>\n \
		\t\t\t\t<coordinates>"
	
	return $kmlHeader
}


# getKmlCoordinateFooter
# returns Google Earth .kml Coordinate footer information as string
# including formatting for easy reading
# .kml file made up of 'header', co-ordinate data and 'footer'
proc ::sap::getKmlCoordinateFooter {} {
	
	set kmlFooter "\t\t\t\t</coordinates>\n \
        	\t\t\t</LineString>\n \
        	\t\t</Placemark>"
	
	return $kmlFooter
}


# getKmlFooter
# returns Google Earth .kml footer information as string
# including formatting for easy reading
# .kml file made up of 'header', co-ordinate data and 'footer'
proc ::sap::getKmlFooter {} {
	
	set kmlFooter "\t</Document>\n \
        	</kml>	"
	
	return $kmlFooter
}

# exportCSV
# writes log data into formatted csv file
# for easy import into external applications for analysis
# currently works with excel
#TODO - pad with "," to form a universally compatible csv file
proc ::sap::exportCsv {} {

	variable S
	# test and return if there is no data to export
	if {[llength $S(log.Data)] <= 0} {
		set answer [tk_messageBox -title "Error" \
                	-message "No data available to export" \
                	-type ok ]
		return
	}
	
	# get file name	
	set filename [::sap::select_save_file $S(log.FileTypesCSV) ".csv"]
	
	# check for user cancel 
	if { $filename == "" } {
            	return
        }
	
	# perform idle system tasks (prevents 'hanging window') 
	update
	
	#create lists for each packet type
	#add header/title information
	
	#AP Attitude (debug + servo position) packets
	set apAttitudeData [list "Ardupilot,Attitude,data,,,,,,,,"]
	lappend apAttitudeData "ASP:,THH:,RLL:,PCH:, \
		CRS:,IMU:,ch3_in:,ch1_out:,ch2_out:,ch3_out:,timestamp"
	
#	set apAttitudeData [list]
	
	
	#AP telemetry packets
	set apPositionData [list "Ardupilot,Position,data,,,,,,,,,,"]
	lappend apPositionData "LAT:,LON:,SPD:,CRT:,ALT:,ALH:,CRS:,BER:,WPN:,DST:,BTV:,RSV:,TOW:,timestamp"
	#
	set fgIMUData [list "FlightGear,IMU,Data,"]
	lappend fgIMUData "roll,pitch,yaw,timestamp"
	#
	set fgGPSData [list "FlightGear,GPS,Data,,,,"]
	lappend fgGPSData "latitude,longitude,altitude,speed,course,iTOW/timestamp,packet "
	
	#split log file into packet types
	foreach x $S(log.Data) {
		
		# AP Attitude packets - already in csv format so just strip +++ header
		if {[string range $x 0 2] == "+++" } {
#			lappend apAttitudeData [string range $x 4 end]			
		
		#split string into component parts
		set attList [split $x ",:"]
		#pull out integer (data) values
		set attDataList [list]
		foreach i $attList {
			if { [string is integer $i] } {
				lappend attDataList $i					
			}
		}
		#pack data into a CSV string and save
		lappend apAttitudeData [join $attDataList ","]			
	}
		
		# AP Position packets
		if {[string range $x 0 2] == "!!!" } {
			
			#split string into component parts
			set posList [split $x ",:"]
						
			#pull out integer (data) values
			set posDataList [list]
			foreach i $posList {
				if { [string is integer $i] } {
					lappend posDataList $i					
				}
			}
			#pack data into a CSV string and save
			lappend apPositionData [join $posDataList ","]			 
		}
		
		# SAP IMU packets - already in csv format so just strip IMU header
		if {[string range $x 0 3] == "IMU," } {
			lappend fgIMUData [string range $x 4 end]			
		}
		
		# SAP GPS packets - already in csv format so just strip GPS header
		if {[string range $x 0 3] == "GPS," } {
			lappend fgGPSData [string range $x 4 end]			
		}
	}
	
	#combine into csv
	
	# open file
	set chan [open $filename w]
        fconfigure $chan -buffering none
        
	# write coordinate data to file - order of strings should be by descending data list length
	foreach j $fgIMUData k $apAttitudeData l $fgGPSData m $apPositionData {
		puts $chan "$j,,$k,,$l,,$m"		
	}
	
	# close file
	flush $chan
        close $chan	
}


# log_Startup
# logs startup date/time, version, and timestamp reference 
#
# Outputs:
#  reference data in system log variable
proc ::sap::log_Startup {} {
	variable S
	
	lappend S(log.Data) "SAP ArduPilot Simulation logfile \n"
	lappend S(log.Data) "SAP Version:- $S(sap_version) \n"
	
	lappend S(log.Data) "Startup Time:- [clock format [clock scan now] ]"
	
	set referenceTimeStamp [expr { int( [clock milliseconds] ) }]
	lappend S(log.Data) "Startup reference timestamp:- $referenceTimeStamp \n"	
}
